#!/usr/bin/env python3
# -*- coding: utf-8 -*-
from __future__ import absolute_import
from __future__ import print_function
import rospy
import csv
import math
# from nav_msgs.msg import Odometry
from std_msgs.msg import Float32MultiArray, Int32
import os
import sys
# add path: /home/chiweiming/catkin_ws/src/cwm_ros
sys.path.append(os.path.dirname(os.path.abspath(os.path.dirname(__file__))))

#---Directory Path---#
# Path: /home/chiweiming/catkin_ws/src/cwm_ros/script
dirPath = os.path.dirname(os.path.realpath(__file__))

# 设置输出的名字
savename = 'yzg_hand_B'

# Number of experiment
numexp = '1'

def myhook():
    f.close()
    print ("shutdown time!")

def leaderOdom_CB(data):
    # 控制接收频率 
    rate = rospy.Rate(10)
    rate.sleep()

    if ((abs(data.data[2]) <= 0.3) and (abs(data.data[3]) <= 1.0)):
            # 创建列表用于存储所需数据: 领航者的位置（x，y） 速度（v，w）
        a = []
        a.append(data.data[0])
        a.append(data.data[1])
        a.append(data.data[2])
        a.append(data.data[3])
        a.append(data.data[4])
        a.append(data.data[5])
        a.append(data.data[6])
        a.append(data.data[7])
        a.append(data.data[8])
        a.append(data.data[9])
        a.append(data.data[10])
        a.append(data.data[11])
        a.append(data.data[12])
        a.append(data.data[13])
        a.append(data.data[14])
        a.append(data.data[15])
        a.append(data.data[16])
        a.append(data.data[17])

        print(a)
        
        # 写入csv文件内容
        csv_writer.writerow(a)


def listenTOLeader():
    # 初始化节点
    rospy.init_node('listenTOLeader')
    # 创建接收器
    rospy.Subscriber('/publishAllLocations', Float32MultiArray, leaderOdom_CB, queue_size=1, buff_size=52428800)
    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    f = open(dirPath + '/ExperimentalData/' + savename + '-' + numexp + '.csv','w')
    csv_writer = csv.writer(f)
    csv_writer.writerow(["Leader X","Leader Y","Leader V_x","Leader W",\
        "follower1 X","follower1 Y","follower1 V_x","follower1 W",\
        "follower2 X","follower2 Y","follower2 V_x","follower2 W",\
        "state"])
    listenTOLeader()
    rospy.on_shutdown(myhook)
